#include <stdlib.h>
#include <time.h>
#include <math.h>

static double** gMatrix;
static double** backupMatrix;
static double* backupVector;

void makeMatrix(int n) {
  gMatrix = (double**)malloc(n * sizeof(double*));
  backupMatrix = (double**)malloc(n * sizeof(double*));
  backupVector = (double*)malloc(n * sizeof(double));
  int i, j;
  i = 0;
  for(i;i<n;i++) {
    gMatrix[i] = (double*)malloc((n+1) * sizeof(double));
    backupMatrix[i] = (double*)malloc(n * sizeof(double));
  }
  srand48(time(0));
  i = 0;
  for(i;i<n;i++) {
    j = 0;
    for(j;j<n+1;j++) {
      gMatrix[i][j] = drand48();
      if (j == n)
	backupVector[i] = gMatrix[i][j];
      else 
	backupMatrix[i][j] = gMatrix[i][j];
    }
  }
}

void freeMatrix(int n) {
  int i = 0;
  for(i;i<n;i++) {
    free(gMatrix[i]);
    free(backupMatrix[i]);
  }
  free(gMatrix);
  free(backupMatrix);
  free(backupVector);
}

void printMatrix(int n) {
  int i,j;
  i = 0;
  for(i;i<n;i++) {
    j = 0;
    for(j;j<n+1;j++) {
      printf("%e ", gMatrix[i][j]);
      if(j==n-1)
	printf("\t");
    }
    printf("\n");
  }
}

void solveLinear(int n) {
  int i, j, k;
  double m;
  double* tempRow;
  double tempValue;
  for(i=0;i<n;i++) {
    //Is swapping really needed?
    /* 
    int pivotRow = -1;
    for(j=0;j<n;j++) {
      if(gMatrix[i][j] > 0.00001) {
	pivotRow = j;
	break;
      }
    }
    printf("Pivot: %d\n", pivotRow);
    if(i!=pivotRow) {
      tempRow = gMatrix[i];
      gMatrix[i] = gMatrix[pivotRow];
      gMatrix[pivotRow] = tempRow;
      tempValue = gVector[i];
      gVector[i] = gVector[pivotRow];
      gVector[pivotRow] = tempValue;
    }
    */
    j = 0;
    m = 1.0/gMatrix[i][i];
    for(j;j<n+1;j++)
      gMatrix[i][j] *= m;
    j=0;
    for(j;j<n;j++) {
      if(i==j)
	continue;
      m = gMatrix[j][i]/gMatrix[i][i];
      k = 0;
      for(k;k<n+1;k++)
	gMatrix[j][k] -= m * gMatrix[i][k];
    }
  }
}

void pSolveLinear(int n, int p) {

}

double verify(int n) {
  int i, j;
  i = 0;
  double SSR, rowSum;
  SSR = 0.0;
  for(i;i<n;i++) {
    rowSum = 0.0;
    j = 0;
    for(j;j<n;j++)
      rowSum += backupMatrix[i][j] * gMatrix[j][n];
    rowSum -= backupVector[i];
    SSR += rowSum * rowSum;
  }
  printf("SSR: %e\n", sqrt(SSR));
}

void main(int argc) {
  int n = 10;
  makeMatrix(n);
  printf("<Before>\n");
  printMatrix(n);
  solveLinear(n);
  printf("<After>\n");
  printMatrix(n);
  verify(n);
  freeMatrix(n);
}
